package environment;

import java.awt.Point;
import java.awt.geom.Point2D;

public class RobotHelper 
{
	public static double w1 = -100;
	public static double w2 = 50;
	public static double w3 = -3;
	public static double w4 = -3;
	public static double w5 = +25;
	
	/** Obstacle weights, used in obstacle factor calculation **/
	static double owHigh = 0.75, owMedium = 0.5, owLow = 0.25;
	
	public static int calculateBindingAffinity(Robot r, Box b)
	{
		double ba = 0;
		double ds = RobotHelper.calculateDistance(r,b);
		double sr = RobotHelper.calculateSuccessRate(r,b);
		double or = RobotHelper.calculateOrientationDiff(r,b);
		double ob = RobotHelper.calculateObstacleFactor(r,b);
		
		/** added velocity factor **/
		double vf = r.velocity;
		
		ba += w1 * ds + w2 * sr + w3 * or + w4 * ob + w5 * vf;
		
		return (int)ba;
	}
	
	/**
	 * Calculates the distance between the center of a robot and a box
	 * The best solution would be to calculate the distance between
	 * the robot's head and the closest point to a box
	 * @param r
	 * @param b
	 * @return
	 */
	private static double calculateDistance(Robot r, Box b)
	{
		/** Toar: being lazy and use Java's method **/
		return Point2D.distance(r.getX(), r.getY(), b.getX(), b.getY());
	}
	
	private static double calculateSuccessRate(Robot r, Box b)
	{
		String key = r.getID() + b.getID();
		return Whiteboard.getSuccessRate(key);
	}
	
	private static double calculateOrientationDiff(Robot r, Box b)
	{
		double angle = getGoalAngle(r, new Point(b.getX(), b.getY()));
		/** Robot can turn left or right, therefore ignore angle > 180 **/
		return angle % 180;
	}
	
	private static double calculateObstacleFactor(Robot r, Box b)
	{
		double angle = getGoalAngle(r, new Point(b.getX(), b.getY()));
		double ob = 0;
		if(angle > 340 && angle < 20)
		{
			/** Robot needs to move forward **/
			/** less value for right or left obstacle **/
			if(r.sonar.s1Fire)
				ob += owHigh;
			if(r.sonar.s2Fire)
				ob += owMedium;
			if(r.sonar.s3Fire)
				ob += owMedium;
		}
		else if(angle >= 20 && angle < 180)
		{
			/** Robot needs to turn right **/
			/** little value for left obstacle, medium value for center obstacle **/
			if(r.sonar.s1Fire)
				ob += owMedium;
			if(r.sonar.s2Fire) /** RIGHT SONAR **/
				ob += owHigh;
			if(r.sonar.s3Fire) /** LEFT SONAR **/
				ob += owLow;
		}
		else
		{
			/** Robot needs to turn left **/
			/** little value for right obstacle, medium value for center obstacle **/
			if(r.sonar.s1Fire)
				ob += owMedium;
			if(r.sonar.s2Fire) /** RIGHT SONAR **/
				ob += owLow;
			if(r.sonar.s3Fire) /** LEFT SONAR **/
				ob += owHigh;
		}
		return ob;
	}
	
	protected static double getGoalAngle(Robot r, Point goal)
	{
        Point goalLocalPosition=new Point(0,0);
        frameConvert(goal, goalLocalPosition, new Point(r.getX(),r.getY()), r.getAngle());
        double goalAngle=(Math.atan2(goalLocalPosition.y, goalLocalPosition.x))/Math.PI*180;
        goalAngle += 90;
        while(goalAngle<0)
            goalAngle=goalAngle+360;
		return goalAngle;
	}
	
    private static void frameConvert(Point p1, Point p2, Point origin, double angle)
    {  
    	// convert the global coordinate into local coordinate
        p2.x=(int) (p1.x*Math.cos(angle/180*Math.PI)+p1.y*Math.sin(angle/180*Math.PI)
             -origin.x*Math.cos(angle/180*Math.PI)-origin.y*Math.sin(angle/180*Math.PI));
        p2.y=(int) (p1.y*Math.cos(angle/180*Math.PI)-p1.x*Math.sin(angle/180*Math.PI)
             +origin.x*Math.sin(angle/180*Math.PI)-origin.y*Math.cos(angle/180*Math.PI));
    }  
}
